﻿// MatrixWrapperOpenCV
// Wrapper of OpenCV matrix functionality
// 
// FLMScan - Featured by FabLab Munich

using System;
using System.Runtime.InteropServices;
using FLMScan;
using FLMScan.Geometry;
using System.Collections.Generic;

namespace FLMScan.OpenCV
{
    class MatrixWrapperOpenCV
    {
        private const string CXCORE_LIBRARY = ".\\3rdParty\\OpenCV\\cxcore210.dll";
        private const string CV_LIBRARY = ".\\3rdParty\\OpenCV\\cv210.dll";        
        
        public static FlmMatrix MatrixMultiply(FlmMatrix rotation, FlmMatrix ledWorldPoints, FlmMatrix shiftMatrix)
        {
            CvMat rotMatrix = rotation.AsCvMat();
            CvMat worldPointMatrix = ledWorldPoints.AsCvMat();
            CvMat shift = shiftMatrix.AsCvMat();
            CvMat ret = CvCreateMat(3, 4, OpenCVConstants.CV_64FC1);

            // multiplies rotation matrix, world point matrix and translation matrix
            // returns result in matrix ret
            CvMatMulAdd(ref rotMatrix, ref worldPointMatrix, ref shift, ref ret);

            FlmMatrix result = FlmMatrix.FromCvMat(ret);

            CvReleaseMat(ref rotMatrix);
            CvReleaseMat(ref worldPointMatrix);
            CvReleaseMat(ref shift);
            CvReleaseMat(ref ret);

            return result;
        }

        public static FlmMatrix Solve(FlmMatrix a, FlmMatrix b)
        {
            CvMat A = a.AsCvMat();
            CvMat B = b.AsCvMat();
            CvMat X = CvCreateMat(3, 1, OpenCVConstants.CV_64FC1);

            // solve minimization problem (Ax=b) for x.
            CvSolve(ref A, ref B, ref X, OpenCVConstants.CV_SVD);
            FlmMatrix matX = FlmMatrix.FromCvMat(X);

            CvReleaseMat(ref A);
            CvReleaseMat(ref B);
            CvReleaseMat(ref X);

            return matX;
        }
                
        public static CvMat CvCreateMat(int rows, int cols, int type)
        {
            IntPtr p = cvCreateMat(rows, cols, type);
            CvMat i = (CvMat)Marshal.PtrToStructure(p, typeof(CvMat));
            i.ptr = p;
            return i;
        }
        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern IntPtr cvCreateMat(int rows, int cols, int type);

        public static void CvReleaseMat(ref CvMat mat)
        {
            cvReleaseMat(ref mat.ptr);
        }
        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvReleaseMat(ref IntPtr mat);        
              
        public static double CvGetReal2D(ref CvMat arr, int idx0, int idx1)
        {
            return cvGetReal2D(ref arr, idx0, idx1);
        }

        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern double cvGetReal2D(ref CvMat arr, int idx0, int idx1);

        public static void CvSetReal2D(ref CvMat arr, int idx0, int idx1, double value)
        {
            cvSetReal2D(ref arr, idx0, idx1, value);
        }
        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvSetReal2D(ref CvMat arr, int idx0, int idx1, double value);

        public static void CvSet2D(ref CvMat arr, int idx0, int idx1, CvScalar value)
        {
            cvSet2D(ref arr, idx0, idx1, value);
        }
        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvSet2D(ref CvMat arr, int idx0, int idx1, CvScalar value);

        public static CvScalar CvGet2D(ref CvMat arr, int idx0, int idx1)
        {
            return cvGet2D(ref arr, idx0, idx1);
        }
        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern CvScalar cvGet2D(ref CvMat arr, int idx0, int idx1);

        private static void CvMatMulAdd(ref CvMat src1, ref CvMat src2, ref CvMat src3, ref CvMat dst)
        {
            cvGEMM(ref src1, ref src2, 1, ref src3, 1, ref dst, 0);
        }

        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvGEMM(ref CvMat src1, ref CvMat src2, double alpha, ref CvMat src3, double beta, ref CvMat dst, int tABC);

        private static int CvSolve(ref CvMat A, ref CvMat B, ref CvMat X, int method)
        {
            return cvSolve(ref A, ref B, ref X, method);
        }
        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern int cvSolve(ref CvMat A, ref CvMat B, ref CvMat X, int method);
        
        private static void CvUndistort2(ref CvMat src, ref CvMat dst, ref CvMat intrinsic_matrix, ref CvMat distortion_coeffs)
        {
            cvUndistort2(ref src, ref dst, ref intrinsic_matrix, ref distortion_coeffs);
        }        
        [DllImport(CV_LIBRARY)]
        private static extern void cvUndistort2(ref CvMat src, ref CvMat dst, ref CvMat intrinsic_matrix, ref CvMat distortion_coeffs);                

        public static void CvSetZero(ref CvMat arr)
        {
            cvSetZero(ref arr);
        }

        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvSetZero(ref CvMat arr);

        public static void CvSet(ref CvMat arr, CvScalar value)
        {
            cvSet(ref arr, value, IntPtr.Zero);
        }
        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvSet(ref CvMat arr, CvScalar value, IntPtr mask);

        public static CvMat CvGetCols(ref CvMat arr, ref CvMat submat, int start_col, int end_col)
        {
            IntPtr p = cvGetCols(ref arr, ref submat, start_col, end_col);
            CvMat i = (CvMat)Marshal.PtrToStructure(p, typeof(CvMat));
            i.ptr = p;
            return i;
        }
        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern IntPtr cvGetCols(ref CvMat arr, ref CvMat submat, int start_col, int end_col);        
    }
}
